﻿#include "cylinder.h"
#include "CSerialPort.h"

extern CSerialPort myserialport;
char data[8];
short x, z;
float y_min;
typedef pcl::PointXYZ PointT;

pcl::PassThrough<PointT> pass;                         
pcl::VoxelGrid<PointT> sor;

pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
pcl::EuclideanClusterExtraction<PointT> ec;

pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);

pcl::console::TicToc tt;
SimpleOpenNIViewer::SimpleOpenNIViewer() : viewer("PCL OpenNI Viewer") {}  // Construct a cloud viewer, with a window name

													   // 定义回调函数cloud_cb_,获取到数据时对数据进行处理
void SimpleOpenNIViewer::cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
{
	if (!viewer.wasStopped())
	{
		tt.tic();
		data[0] = 1;
		int times = 0;
		short pre_data[10][3];//0 x;1 z;2 size;  int *p

							  // Build a passthrough filter to remove spurious NaNs

		pass.setInputCloud(cloud);
		pass.setFilterFieldName("y");
		pass.setFilterLimits(-0.15, 0.15);
		pass.filter(*cloud_filtered);

		sor.setInputCloud(cloud_filtered);
		sor.setLeafSize(0.004f, 0.008f, 0.005f);
		sor.filter(*cloud_filtered);
		//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;

		tree->setInputCloud(cloud_filtered);

		std::vector<pcl::PointIndices> cluster_indices;
		ec.setClusterTolerance(0.012); // 1.3cm
		ec.setMinClusterSize(500);
		ec.setMaxClusterSize(1300);
		ec.setSearchMethod(tree);
		ec.setInputCloud(cloud_filtered);
		ec.extract(cluster_indices);

		pcl::PointCloud<pcl::PointXYZ>::Ptr all_cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		all_cloud_cluster->resize(cloud_filtered->size());
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
			for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
				cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;

			pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
			feature_extractor.setInputCloud(cloud_cluster);
			feature_extractor.compute();

			pcl::PointXYZ min_point_AABB;
			pcl::PointXYZ max_point_AABB;
			feature_extractor.getAABB(min_point_AABB, max_point_AABB);

			if (z > 0.4)y_min = 0.295;
			else y_min = 0.28;
			if (((max_point_AABB.x - min_point_AABB.x)>0.06) && ((max_point_AABB.x - min_point_AABB.x)<0.115) && ((max_point_AABB.z - min_point_AABB.z)<0.11) && ((max_point_AABB.y - min_point_AABB.y)>y_min))
			{
				all_cloud_cluster->operator+=(*cloud_cluster);
				pre_data[times][0] = ((max_point_AABB.x + min_point_AABB.x) / 2) * 1000;
				pre_data[times][1] = min_point_AABB.z * 100;
				pre_data[times][2] = cloud_cluster->size();
				times++;
				std::cout << max_point_AABB.x - min_point_AABB.x << "  " << max_point_AABB.z - min_point_AABB.z << "  " << max_point_AABB.y - min_point_AABB.y << "  " << ((max_point_AABB.x + min_point_AABB.x) / 2) * 100 << "  " << min_point_AABB.z * 100 << "        size: " << cloud_cluster->points.size() << std::endl;
			}
			//std::cout << "                       size: " << cloud_cluster->points.size() << " data points." << std::endl;

		}
		if (times == 0)
		{
			x = z = 0;
			data[0] = 0;
		}
		else if (times == 1)
		{
			x = pre_data[0][0];
			z = pre_data[0][1];
		}
		else
		{
			short temp_x, temp_z, temp_size, temp_min_size, max_n, min_n;
			temp_x = temp_z = temp_size = temp_min_size = max_n = min_n = 0;
			for (int i = 0; i < times; i++)
			{
				if (pre_data[i][2] > temp_size)
				{
					temp_min_size = temp_size;
					min_n = max_n;
					temp_size = pre_data[i][2];
					max_n = i;
				}
			}
			if (temp_size - temp_min_size > 100)
			{
				x = pre_data[max_n][0];
				z = pre_data[max_n][1];
			}
			else
			{
				if (abs(pre_data[max_n][0]) < abs(pre_data[min_n][0]))
				{
					x = pre_data[max_n][0];
					z = pre_data[max_n][1];
				}
				else
				{
					x = pre_data[min_n][0];
					z = pre_data[min_n][1];
				}
			}
		}
		std::cout << x / 10 << "  " << z;
		
		data[1] = x;
		data[2] = (x >> 8);
		data[3] = z;
		data[4] = (z >> 8);
		data[5] = 0;
		data[6] = 0;
		data[7] = 0x5A;
		//myserialport.writeToCom_char((char*)&data, sizeof(data));

		/*
		viewer.showCloud(all_cloud_cluster);
		std::cout << std::endl;

		ne.setNumberOfThreads(8);
		ne.setSearchMethod(tree);
		ne.setInputCloud(all_cloud_cluster);
		ne.setRadiusSearch(0.08);
		ne.compute(*cloud_normals);

		// Create the segmentation object for cylinder segmentation and set all the parameters
		//seg.setOptimizeCoefficients(true);
		seg.setModelType(pcl::SACMODEL_CYLINDER);
		seg.setMethodType(pcl::SAC_RANSAC);
		seg.setNormalDistanceWeight(0.1);
		seg.setMaxIterations(5000);
		seg.setDistanceThreshold(0.05);
		seg.setRadiusLimits(0.02, 0.05);
		seg.setInputCloud(all_cloud_cluster);
		seg.setInputNormals(cloud_normals);

		// Obtain the cylinder inliers and coefficients
		seg.segment(*inliers_cylinder, *coefficients_cylinder);
		std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers_cylinder);
		extract.setNegative(false);

		pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
		extract.filter(*cloud_cylinder);
		cout << cloud_cylinder->points.size() << endl;
		if (cloud_cylinder->points.size() > 0)
		{
		x = (coefficients_cylinder->values[0] * 1000);
		z = coefficients_cylinder->values[2] * 100;

		}
		*/
		/*
		CSerial mySerialPort;

		if (!mySerialPort.OpenSerialPort(_T("COM3:"), 115200, 8, 1))
		{
		std::cout << "initPort fail !" << std::endl;
		}
		else
		{

		std::cout << "initPort success !" << std::endl;
		if (times>0)
		{
		data[0] = 1;
		data[1] = x;
		data[2] = (x >> 8);
		data[3] = z;
		data[4] = (z >> 8);
		}
		else
		{
		data[0] = 0;
		data[1] = 0;
		data[2] = 0;
		data[3] = 0;
		data[4] = 0;
		}
		if(mySerialPort.SendData(data, 8))
		std::cout << "success write" << std::endl;

		}
		*/
		std::cerr << "    Done: " << tt.toc() << " ms\n\n";

		//system("pause");
		if (all_cloud_cluster->size()>0)viewer.showCloud(all_cloud_cluster);
	}
}

void SimpleOpenNIViewer::run()
{
	// create a new grabber for OpenNI devices
	pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

	// make callback function from member function
	boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
		boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);

	// connect callback function for desired signal
	boost::signals2::connection c = interface->registerCallback(f);

	// start receiving point clouds
	interface->start();

	while (!viewer.wasStopped())
	{
		boost::this_thread::sleep(boost::posix_time::seconds(0.001));
		if (mode == 0)continue;
		else break;
	}
	/*
	while (!viewer.wasStopped()&&flag!=1)
	{
	boost::this_thread::sleep(boost::posix_time::seconds(1));
	}
	*/
	// Stop the data acquisition
	interface->stop();
}

